
public class FormationMotionCalculator {
	/**
	 * @param hdg - heading of robot (gotten from sensors) in global coordinates
	 * @param anc - position of anchor (gotten from sensors) in robot coordinates
	 * @param ahd - heading of anchor (gotten from sensors???) in robot coordinates
	 * @param apl - predefined location of anchor in leader coordinates
	 * @param mpl - predefined location of robot in leader coordinates
	 * @return vector from robot to target position in robot coordinates
	 */
	public Vec2D move(
		double hdg,
		Vec2D anc,
		double ahd,
		Vec2D apl,
		Vec2D mpl
	){
		double nhd=0, dst=0;
		double lhd = ahd+hdg; //leader heading (in global coordinates) gotten from anchor heading
		Vec2D mpa = mpl.sub(apl); //robot target position in anchor coordinates
		
		Vec2D mpt = anc.rotated(hdg).add(mpa.rotated(lhd)); // robot target position in global coordinates, but related to robot position 
		
		nhd = mpt.h() - hdg;
		dst = mpt.r();
		
		return Vec2D.polar(nhd, dst); // robot target position in robot coordinates
	}

	
	
public static void main(String args[]){
	FormationMotionCalculator fmc = new FormationMotionCalculator();
	
	//Formation
	final Vec2D apl = Vec2D.polar(Vec2D.d2r(180+45), 3);
	final Vec2D mpl = Vec2D.polar(Vec2D.d2r(180+90), 5);
	
	
	System.out.println("leader_x leader_y anchor_x anchor_y me_x me_y moved_x moved_y");
	System.out.println();
	Vec2D prev = null;
	double my_heading = 0; 
	for(int i=0;i<360;i+=10){
		double leader_heading = Vec2D.d2r(i);
		Vec2D leader = Vec2D.polar(Vec2D.d2r(45),30).add( Vec2D.polar(leader_heading, 15) );	
		Vec2D anchor = leader.add(apl.rotated(leader_heading));
		Vec2D me = leader.add(mpl.rotated(leader_heading));
		
		Vec2D moved = me;
		if(prev!=null){
			//OSERVATION
			//double my_heading = prev.h();
			double anchor_heading = leader_heading -my_heading;
			Vec2D anchor_pos = anchor.sub(prev).rotated( -my_heading);
//			System.out.println("anc="+Vec2D.r2d(anchor_pos.h())+","+anchor_pos.r());//+"   mpa="+Vec2D.r2d(mpa.t())+","+anc.r());
//			System.out.println("h="+Vec2D.r2d(my_heading)+"");
			
			Vec2D move = fmc.move(my_heading, anchor_pos, anchor_heading, apl, mpl );		
//			Vec2D move = fmc.move1(anchor_pos, apl, mpl );
			
			Vec2D vv = move.rotated( +my_heading);
			moved = prev.add(move.rotated( +my_heading));
			
			my_heading = move.rotated( +my_heading).h();
			prev = moved;
//			System.out.println("move="+Vec2D.r2d(vv.h())+","+vv.r());
//			System.out.println("----");
		}else{
			prev = me;
		}
		
		System.out.println(leader.x()+" "+leader.y()+" "+anchor.x()+" "+anchor.y()+" "+me.x()+" "+me.y()+" "+moved.x()+" "+moved.y());

	}
	 
	
	
}
	
	
	
}
